package foundation.graph;

import java.util.Arrays;
import java.util.PriorityQueue;

public class AStar {

    public static int[] move = {-1, 0, 1, 0, -1};
    /*
       grid[i][j] == 0 代表障碍
       grid[i][j] == 1 代表道路
       返回从(startX,startY) -> (targetX,targetY) 的最短路径
     */

    private static int minDistance(int[][] grid, int startX, int startY, int targetX, int targetY) {
        if (grid[startX][startY] == 0 || grid[targetX][targetY] == 0) {
            return -1;
        }
        int n = grid.length;
        int m = grid[0].length;
        int[][] dist = new int[n][m];
        for (int i = 0; i < n; i++) {
            for (int j = 0; j < m; j++) {
                dist[i][j] = Integer.MAX_VALUE;
            }
        }
        dist[startX][startY] = 1;
        boolean[][] visited = new boolean[n][m];
        /*
          0: 行
          1: 列
          2: 从源点到当前点的距离 + 当前点到终点的预估距离
         */
        PriorityQueue<int[]> heap = new PriorityQueue<>((a, b) -> a[2] - b[2]);
        heap.add(new int[]{startX, startY, 1 + f(startX, startY, targetX, targetY)});
        while (!heap.isEmpty()) {
            int[] cur = heap.poll();
            int x = cur[0];
            int y = cur[1];
            if (visited[x][y]) continue;
            visited[x][y] = true;
            if (x == targetX && y == targetY) {
                return dist[x][y];
            }
            for (int i = 0, nx, ny; i < 4; i++) {
                nx = x + move[i];
                ny = y + move[i + 1];
                if (nx >= 0 && nx < n && ny >= 0 && ny < m
                        && grid[nx][ny] == 1
                        && !visited[nx][ny]
                        && dist[nx][ny] > dist[x][y] + 1) {
                    dist[nx][ny] = dist[x][y] + 1;
                    heap.add(new int[]{nx, ny, dist[nx][ny] + f(nx, ny, targetX, targetY)});
                }
            }
        }
        return -1;

    }

    private static int f(int x, int y, int targetX, int targetY) {
        return Math.abs(x - targetX) + Math.abs(y - targetY);
    }

    public static void main(String[] args) {
        int[][] grid = {
                {1, 0, 0, 1},
                {1, 1, 0, 1},
                {1, 1, 1, 1}
        };
        int startX = 0;
        int startY = 0;
        int targetX = 0;
        int targetY = 3;
        System.out.println(minDistance(grid, startX, startY, targetX, targetY));
    }


}
